#ifdef KITSCHY_DEBUG_MEMORY 
#include "debug_memorymanager.h"
#endif

#ifdef WIN32
#include "windows.h"
#else
#include <sys/time.h>
#include <time.h>
#endif
#include "stdio.h"
#include "stdlib.h"
#include "string.h"
#include "stdarg.h"
#include "math.h"

#include "BList.h"
#include "Pair.h"

#include "symbol.h"
#include "sort.h"
#include "ontology.h"
#include "featureterm.h"
#include "FTKBase.h"
#include "AUfeatureterm.h"
#include "FT_refinement.h"
#include "FT_generalization.h"

#include "Planning.h"
#include "SearchBasedPlanning.h"


List<FeatureTerm> *IDSearchBasedPlanAuxiliar(List<FeatureTerm> *objects,FeatureTerm *initialState,FeatureTerm *goalState,FTKBase *domain_model,Ontology *o,int maxDepth);
float heuristic(FeatureTerm *state,FeatureTerm *goal,FTKBase *dm);
float heuristicNegated(FeatureTerm *state,FeatureTerm *goal,FTKBase *dm);
bool HSearchBasedPlanSortOrder(SBPNode *n1,SBPNode *n2);

int g_nodesEvaluatedInLastSearch = 0;


List<FeatureTerm> *IDSearchBasedPlan(List<FeatureTerm> *objects,FeatureTerm *initialState,FeatureTerm *goalState,FTKBase *domain_model,Ontology *o)
{
	List<FeatureTerm> *plan = 0;
	int depth = 1;


	do{
		g_nodesEvaluatedInLastSearch = 0;
		printf("Trying Depth %i\n",depth);
		plan = IDSearchBasedPlanAuxiliar(objects,initialState,goalState,domain_model,o,depth);
		depth++;
		printf("Expanded %i nodes\n",g_nodesEvaluatedInLastSearch);
	}while(plan==0);


	return plan;
} /* SearchBasedPlan */ 


List<FeatureTerm> *IDSearchBasedPlanAuxiliar(List<FeatureTerm> *objects,FeatureTerm *initialState,FeatureTerm *goalState,FTKBase *domain_model,Ontology *o,int maxDepth)
{
	List<FeatureTerm> *operators;
	FeatureTerm *op;

	if (maxDepth<=0) {
		g_nodesEvaluatedInLastSearch++;
		if (evaluatePredicate(goalState,initialState,domain_model)) {
			return new List<FeatureTerm>;
		} else {
			return 0;
		} // if
	} else {
		operators = validInstantiations(domain_model,objects,initialState);

		operators->Rewind();
		while(operators->Iterate(op)) {
			FeatureTerm *newState = applyOperator(op,initialState,domain_model);
/*
			{
				char *s1 = initialState->toStringNOOS(domain_model);
				char *s2 = newState->toStringNOOS(domain_model);
				printf("Applying Operator:\n");
				printOperatorNice(op,domain_model);
				printf("Initial State:\n%s\n",s1);
				printf("Final State:\n%s\n",s2);
				delete []s1;
				delete []s2;
			}
*/
			List<FeatureTerm> *plan = IDSearchBasedPlanAuxiliar(objects,newState,goalState,domain_model,o,maxDepth-1);
			if (plan!=0) {
				plan->Insert(new FeatureTerm(op));
				delete operators;
				delete newState;
				return plan;
			} // if 

			delete newState;
		} // while 

		delete operators;
		return 0;
	} // if
} /* IDSearchBasedPlan */ 



List<FeatureTerm> *HSearchBasedPlan(List<FeatureTerm> *objects,FeatureTerm *initialState,FeatureTerm *goalState,FTKBase *dm,Ontology *o)
{
	List<SBPNode> m_open,m_closed;
	SBPNode *node,*node2;
	g_nodesEvaluatedInLastSearch = 0;


	node = new SBPNode(new FeatureTerm(initialState),0,0,heuristic(goalState,initialState,dm),0);
	m_open.Add(node);

	while(!m_open.EmptyP()) {
		node = m_open.ExtractIni();
		
		printf("Node: %g + %g\n",node->m_c,node->m_h);

		List<FeatureTerm> *operators = validInstantiations(dm,objects,node->m_state);		
		FeatureTerm *op;

		operators->Rewind();
		while(operators->Iterate(op)) {
			FeatureTerm *newState = applyOperator(op,node->m_state,dm);
/*
			{
				char *s1 = initialState->toStringNOOS(domain_model);
				char *s2 = newState->toStringNOOS(domain_model);
				printf("Applying Operator:\n");
				printOperatorNice(op,domain_model);
				printf("Initial State:\n%s\n",s1);
				printf("Final State:\n%s\n",s2);
				delete []s1;
				delete []s2;
			}
*/
			g_nodesEvaluatedInLastSearch++;
			if (evaluatePredicate(goalState,newState,dm)) {
				// Found plan!!!
				printf("Success with %i expanded nodes\n",g_nodesEvaluatedInLastSearch);
			
				List<FeatureTerm> *res = new List<FeatureTerm>;

				res->Add(new FeatureTerm(op));
				node2 = node;
				while(node2!=0) {
					if (node2->m_op!=0) res->Insert(new FeatureTerm(node2->m_op));
					node2 = node2->m_parent;
				} // while
				delete operators;
				delete node;
				delete newState;
				return res;
			} // if
			node2 = new SBPNode(newState,new FeatureTerm(op),node,heuristic(goalState,newState,dm),node->m_c+1);
			m_open.Add(node2);
		} // while 
		delete operators;

		m_closed.Add(node);

		m_open.Sort(&HSearchBasedPlanSortOrder);
	} // while

	printf("Failure with %i expanded nodes\n",g_nodesEvaluatedInLastSearch);

	return 0;
} /* HIDSearchBasedPlan */ 


float heuristic(FeatureTerm *predicate,FeatureTerm *state,FTKBase *dm)
{
	float h = 0;	

	Ontology *o = predicate->getSort()->getOntology();
	if (predicate->getSort()->is_a(and_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				h+=heuristic(f,state,dm);
			} // while
			return h;
		} else {
			return heuristic(f,state,dm);
		} //
	} else if (predicate->getSort()->is_a(or_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				float htmp = heuristic(f,state,dm);
				if (htmp>h) h = htmp;
			} // while
			return h;
		} else {
			return heuristic(f,state,dm);
		} //
	} else if (predicate->getSort()->is_a(not_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicate");
		return heuristicNegated(f,state,dm);
	} else if (predicate->getSort()->is_a(when_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return heuristic(f,state,dm);
		} else {
			return 0;
		} // if		
	} else if (predicate->getSort()->is_a(unless_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (!evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return heuristic(f,state,dm);
		} else {
			return 0;
		} // if		
	} else if (predicate->getSort()->is_a(if_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("positive-effect");
			return heuristic(f,state,dm);
		} else {
			f = predicate->featureValueQuick("negative-effect");
			return heuristic(f,state,dm);
		} // if		
	} else {
		if (state->getSort()->is_a(and_sort)) {
			FeatureTerm *f1 = state->featureValueQuick("predicates");
			if (!predicate->subsumes(f1)) h++;
			return h;
		} else {
			// Game state is not an and of things... not supported yet...
			printf("heuristic: Game State is not an 'and' construction!;\n");
			char *s = state->toStringNOOS(dm);
			printf("%s\n",s);
			delete []s;
			return h;
		} // if
	} // if
} /* heuristic */ 



float heuristicNegated(FeatureTerm *predicate,FeatureTerm *state,FTKBase *dm)
{	
	float h = 0;

	Ontology *o = predicate->getSort()->getOntology();
	if (predicate->getSort()->is_a(and_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				h+=heuristicNegated(f,state,dm);
			} // while
			return h;
		} else {
			return heuristicNegated(f,state,dm);
		} //
	} else if (predicate->getSort()->is_a(or_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicates");
		if (f->isSet()) {
			List<FeatureTerm> l2;
			l2.Instance(*(f->set_values_quick()));
			while(l2.Iterate(f)) {
				float htmp = heuristicNegated(f,state,dm);
				if (htmp>h) h = htmp;
			} // while
			return h;
		} else {
			return heuristicNegated(f,state,dm);
		} //
	} else if (predicate->getSort()->is_a(not_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("predicate");
		return heuristicNegated(f,state,dm);
	} else if (predicate->getSort()->is_a(when_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return heuristicNegated(f,state,dm);
		} else {
			return 0;
		} // if		
	} else if (predicate->getSort()->is_a(unless_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (!evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("effect");
			return heuristicNegated(f,state,dm);
		} else {
			return 0;
		} // if		
	} else if (predicate->getSort()->is_a(if_sort)) {
		FeatureTerm *f = predicate->featureValueQuick("condition");
		if (evaluatePredicate(f,state,dm)) {
			f = predicate->featureValueQuick("positive-effect");
			return heuristicNegated(f,state,dm);
		} else {
			f = predicate->featureValueQuick("negative-effect");
			return heuristicNegated(f,state,dm);
		} // if		
	} else {
		if (state->getSort()->is_a(and_sort)) {
			FeatureTerm *f1 = state->featureValueQuick("predicates");
			if (predicate->subsumes(f1)) h++;
			return h;
		} else {
			// Game state is not an and of things... not supported yet...
			printf("heuristicNegated: Game State is not an 'and' construction!\n");
			return h;
		} // if
	} // if
} /* heuristicNegated */ 


bool HSearchBasedPlanSortOrder(SBPNode *o1,SBPNode *o2)
{
	return (o1->m_c+o1->m_h)<=(o2->m_c+o2->m_h);
} /* HSearchBasedPlanSortOrder */ 


